#include "app_cmd.h"
#include "module_k230.h"
#include "message_center.h"
#include "task_def.h"
#include "string.h"
#include "buffer.h"
// #include "module_laser.h"
#include "arm_math.h"
#include "OLED.h"
static Publisher_t *chassis_cmd_pub;            
static Subscriber_t *chassis_feed_sub;          
static Publisher_t *gimbal_cmd_pub;            
static Subscriber_t *gimbal_feed_sub;         
static Subscriber_t *cmd_ui_sub;
static Publisher_t *cmd_ui_pub;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      
static Gimbal_Upload_Data_s gimbal_fetch_data; 
static chassis_state_t chassis_fetch_data;
static chassis_control_t chassis_cmd_send;      
static CMD_Ctrl_UI_s cmd_ui_recv;
static CMD_Upload_UI_s cmd_feedback_ui;
// static laser_t *Laser;
static K230_data_t *K230_data;
static float yaw_cmd, pitch_cmd;
static buf_t *buffer_yaw, *buffer_pitch;
static float aligned_total_yaw, aligned_total_pitch;
static float YAKp = 60, YSKp = 40, PAKp = 30, PSKp = 30;

static float pitch_sin[500] = {
9.000000000000000000e+01,
9.037773662995370216e+01,
9.075541337160414912e+01,
9.113297034614312508e+01,
9.151034769375085887e+01,
9.188748558308658687e+01,
9.226432422077446915e+01,
9.264080386088355112e+01,
9.301686481440023613e+01,
9.339244745869160624e+01,
9.376749224695835494e+01,
9.414193971767564051e+01,
9.451573050402039655e+01,
9.488880534328367844e+01,
9.526110508626642570e+01,
9.563257070665738979e+01,
9.600314331039133720e+01,
9.637276414498649046e+01,
9.674137460885945927e+01,
9.710891626061618354e+01,
9.747533082831756701e+01,
9.784056021871819553e+01,
9.820454652647677563e+01,
9.856723204333665933e+01,
9.892855926727531823e+01,
9.928847091162091942e+01,
9.964690991413488064e+01,
1.000038194460588272e+02,
1.003591429211244730e+02,
1.007128240045251601e+02,
1.010648066218473957e+02,
1.014150349679612475e+02,
1.017634535158678943e+02,
1.021100070255032648e+02,
1.024546405524959596e+02,
1.027972994568785339e+02,
1.031379294117503207e+02,
1.034764764118907010e+02,
1.038128867823213994e+02,
1.041471071868163989e+02,
1.044790846363580812e+02,
1.048087664975384428e+02,
1.051361005009037939e+02,
1.054610347492419180e+02,
1.057835177258100430e+02,
1.061034983025026008e+02,
1.064209257479573694e+02,
1.067357497355985885e+02,
1.070479203516161135e+02,
1.073573881028789430e+02,
1.076641039247820686e+02,
1.079680191890255543e+02,
1.082690857113242089e+02,
1.085672557590469722e+02,
1.088624820587847921e+02,
1.091547178038454575e+02,
1.094439166616746775e+02,
1.097300327812018566e+02,
1.100130208001094871e+02,
1.102928358520252488e+02,
1.105694335736352230e+02,
1.108427701117176269e+02,
1.111128021300953463e+02,
1.113794868165068408e+02,
1.116427818893937314e+02,
1.119026456046043734e+02,
1.121590367620121356e+02,
1.124119147120475759e+02,
1.126612393621431067e+02,
1.129069711830895812e+02,
1.131490712153034224e+02,
1.133875010750034562e+02,
1.136222229602964831e+02,
1.138531996571706060e+02,
1.140803945453953077e+02,
1.143037716043274088e+02,
1.145232954186219700e+02,
1.147389311838472565e+02,
1.149506447120027417e+02,
1.151584024369395109e+02,
1.153621714196819710e+02,
1.155619193536501967e+02,
1.157576145697819499e+02,
1.159492260415536720e+02,
1.161367233898996005e+02,
1.163200768880281828e+02,
1.164992574661351625e+02,
1.166742367160124445e+02,
1.168449868955520117e+02,
1.170114809331443979e+02,
1.171736924319707072e+02,
1.173315956741877244e+02,
1.174851656250052940e+02,
1.176343779366555822e+02,
1.177792089522531569e+02,
1.179196357095457870e+02,
1.180556359445549077e+02,
1.181871880951055545e+02,
1.183142713042448406e+02,
1.184368654235488378e+02,
1.185549510163168918e+02,
1.186685093606533030e+02,
1.187775224524355764e+02,
1.188819730081688562e+02,
1.189818444677261624e+02,
1.190771209969739317e+02,
1.191677874902824072e+02,
1.192538295729205799e+02,
1.193352336033352543e+02,
1.194119866753138126e+02,
1.194840766200304358e+02,
1.195514920079754404e+02,
1.196142221507673753e+02,
1.196722571028475670e+02,
1.197255876630569844e+02,
1.197742053760949688e+02,
1.198181025338598999e+02,
1.198572721766711595e+02,
1.198917080943726319e+02,
1.199214048273172608e+02,
1.199463576672326326e+02,
1.199665626579675148e+02,
1.199820165961189957e+02,
1.199927170315404368e+02,
1.199986622677299124e+02,
1.199998513620991787e+02,
1.199962841261231148e+02,
1.199879611253696226e+02,
1.199748836794099134e+02,
1.199570538616093955e+02,
1.199344744987988349e+02,
1.199071491708262585e+02,
1.198750822099893583e+02,
1.198382787003485959e+02,
1.197967444769212335e+02,
1.197504861247561223e+02,
1.196995109778897586e+02,
1.196438271181834239e+02,
1.195834433740419342e+02,
1.195183693190138570e+02,
1.194486152702737485e+02,
1.193741922869863430e+02,
1.192951121685532456e+02,
1.192113874527421302e+02,
1.191230314136989819e+02,
1.190300580598435403e+02,
1.189324821316483138e+02,
1.188303190993015335e+02,
1.187235851602545154e+02,
1.186122972366535180e+02,
1.184964729726569033e+02,
1.183761307316377156e+02,
1.182512895932722472e+02,
1.181219693505150445e+02,
1.179881905064608532e+02,
1.178499742710939557e+02,
1.177073425579253865e+02,
1.175603179805187466e+02,
1.174089238489047773e+02,
1.172531841658858696e+02,
1.170931236232303121e+02,
1.169287675977577550e+02,
1.167601421473156620e+02,
1.165872740066480588e+02,
1.164101905831568473e+02,
1.162289199525564811e+02,
1.160434908544227426e+02,
1.158539326876361599e+02,
1.156602755057210032e+02,
1.154625500120804134e+02,
1.152607875551285730e+02,
1.150550201233205030e+02,
1.148452803400805351e+02,
1.146316014586299161e+02,
1.144140173567147798e+02,
1.141925625312349410e+02,
1.139672720927745502e+02,
1.137381817600355731e+02,
1.135053278541746522e+02,
1.132687472930446830e+02,
1.130284775853415482e+02,
1.127845568246573862e+02,
1.125370236834409923e+02,
1.122859174068665311e+02,
1.120312778066113424e+02,
1.117731452545440618e+02,
1.115115606763237963e+02,
1.112465655449116042e+02,
1.109782018739951326e+02,
1.107065122113276061e+02,
1.104315396319819911e+02,
1.101533277315217276e+02,
1.098719206190888542e+02,
1.095873629104106897e+02,
1.092996997207262666e+02,
1.090089766576334682e+02,
1.087152398138582754e+02,
1.084185357599468489e+02,
1.081189115368821234e+02,
1.078164146486255675e+02,
1.075110930545858139e+02,
1.072029951620147585e+02,
1.068921698183329596e+02,
1.065786663033850630e+02,
1.062625343216267026e+02,
1.059438239942441555e+02,
1.056225858512078162e+02,
1.052988708232609554e+02,
1.049727302338448851e+02,
1.046442157909618658e+02,
1.043133795789770772e+02,
1.039802740503608902e+02,
1.036449520173727592e+02,
1.033074666436881159e+02,
1.029678714359695419e+02,
1.026262202353834994e+02,
1.022825672090641405e+02,
1.019369668415253187e+02,
1.015894739260223787e+02,
1.012401435558649041e+02,
1.008890311156819450e+02,
1.005361922726411450e+02,
1.001816829676228622e+02,
9.982555940635106140e+01,
9.946787805048222708e+01,
9.910869560865351957e+01,
9.874806902749203630e+01,
9.838605548258607314e+01,
9.802271236942033283e+01,
9.765809729427606101e+01,
9.729226806509784353e+01,
9.692528268232855737e+01,
9.655719932971354069e+01,
9.618807636507591496e+01,
9.581797231106423851e+01,
9.544694584587401209e+01,
9.507505579394452866e+01,
9.470236111663254519e+01,
9.432892090286426878e+01,
9.395479435976709226e+01,
9.358004080328254304e+01,
9.320471964876209370e+01,
9.282889040154714166e+01,
9.245261264753466435e+01,
9.207594604373022662e+01,
9.169895030878964803e+01,
9.132168521355087876e+01,
9.094421057155760479e+01,
9.056658622957615989e+01,
9.018887205810707997e+01,
8.981112794189293425e+01,
8.943341377042385432e+01,
8.905578942844242363e+01,
8.867831478644914966e+01,
8.830104969121036618e+01,
8.792405395626978759e+01,
8.754738735246534986e+01,
8.717110959845288676e+01,
8.679528035123790630e+01,
8.641995919671747117e+01,
8.604520564023293616e+01,
8.567107909713574543e+01,
8.529763888336746902e+01,
8.492494420605548555e+01,
8.455305415412600212e+01,
8.418202768893576149e+01,
8.381192363492409925e+01,
8.344280067028648773e+01,
8.307471731767147105e+01,
8.270773193490217068e+01,
8.234190270572396741e+01,
8.197728763057968138e+01,
8.161394451741392686e+01,
8.125193097250797791e+01,
8.089130439134649464e+01,
8.053212194951780134e+01,
8.017444059364893860e+01,
7.981831703237715203e+01,
7.946380772735886922e+01,
7.911096888431806917e+01,
7.875985644413512432e+01,
7.841052607397763552e+01,
7.806303315847469548e+01,
7.771743279093587375e+01,
7.737377976461651485e+01,
7.703212856403048647e+01,
7.669253335631188406e+01,
7.635504798262725501e+01,
7.601972594963912400e+01,
7.568662042102293697e+01,
7.535578420903814845e+01,
7.502726976615512910e+01,
7.470112917673905883e+01,
7.437741414879219803e+01,
7.405617600575585868e+01,
7.373746567837331156e+01,
7.342133369661495124e+01,
7.310783018166705460e+01,
7.279700483798525568e+01,
7.248890694541421453e+01,
7.218358535137443255e+01,
7.188108846311789080e+01,
7.158146424005316533e+01,
7.128476018614173881e+01,
7.099102334236653178e+01,
7.070030027927376182e+01,
7.041263708958932455e+01,
7.012807938091116000e+01,
6.984667226847828658e+01,
6.956846036801802313e+01,
6.929348778867240810e+01,
6.902179812600486741e+01,
6.875343445508841000e+01,
6.848843932367623211e+01,
6.822685474545595241e+01,
6.796872219338865762e+01,
6.771408259313348310e+01,
6.746297631655900773e+01,
6.721544317534262802e+01,
6.697152241465846600e+01,
6.673125270695531697e+01,
6.649467214582534780e+01,
6.626181823996444109e+01,
6.603272790722544983e+01,
6.580743746876507316e+01,
6.558598264328522021e+01,
6.536839854137009809e+01,
6.515471965991947911e+01,
6.494497987667949701e+01,
6.473921244487144122e+01,
6.453744998791960086e+01,
6.433972449427899676e+01,
6.414606731236385428e+01,
6.395650914557725741e+01,
6.377108004744351888e+01,
6.358980941684316690e+01,
6.341272599335194826e+01,
6.323985785268433801e+01,
6.307123240224225924e+01,
6.290687637676968791e+01,
6.274681583411414465e+01,
6.259107615109522271e+01,
6.243968201948126051e+01,
6.229265744207461353e+01,
6.215002572890605848e+01,
6.201180949353914684e+01,
6.187803064948496967e+01,
6.174871040672775990e+01,
6.162386926836227730e+01,
6.150352702734309673e+01,
6.138770276334648202e+01,
6.127641483974548464e+01,
6.116968090069846653e+01,
6.106751786835169327e+01,
6.096994194015645974e+01,
6.087696858630101815e+01,
6.078861254725786978e+01,
6.070488783144676148e+01,
6.062580771301365701e+01,
6.055138472972625152e+01,
6.048163068098614303e+01,
6.041655662595806575e+01,
6.035617288181657614e+01,
6.030048902211024142e+01,
6.024951387524387059e+01,
6.020325552307876649e+01,
6.016172129965140414e+01,
6.012491779001065595e+01,
6.009285082917374154e+01,
6.006552550120116507e+01,
6.004294613839060446e+01,
6.002511632059007951e+01,
6.001203887463037745e+01,
6.000371587387688521e+01,
6.000014863790082131e+01,
6.000133773227008760e+01,
6.000728296845956322e+01,
6.001798340388100428e+01,
6.003343734203248516e+01,
6.005364233276736030e+01,
6.007859517268273919e+01,
6.010829190562736102e+01,
6.014272782332883338e+01,
6.018189746614010005e+01,
6.022579462390503124e+01,
6.027441233694301559e+01,
6.032774289715241878e+01,
6.038577784923261760e+01,
6.044850799202454539e+01,
6.051592337996956417e+01,
6.058801332468619449e+01,
6.066476639666474568e+01,
6.074617042707940584e+01,
6.083221250971759275e+01,
6.092287900302605408e+01,
6.101815553227383049e+01,
6.111802699183113674e+01,
6.122247754756442362e+01,
6.133149063934669698e+01,
6.144504898368310819e+01,
6.156313457645116216e+01,
6.168572869575515227e+01,
6.181281190489443844e+01,
6.194436405544507807e+01,
6.208036429045421301e+01,
6.222079104774682889e+01,
6.236562206334441072e+01,
6.251483437499469886e+01,
6.266840432581227560e+01,
6.282630756802928573e+01,
6.298851906685558788e+01,
6.315501310444798833e+01,
6.332576328398755550e+01,
6.350074253386481615e+01,
6.367992311197180300e+01,
6.386327661010039947e+01,
6.405077395844631383e+01,
6.424238543021805015e+01,
6.443808064634980326e+01,
6.463782858031801482e+01,
6.484159756306047484e+01,
6.504935528799725830e+01,
6.526106881615274347e+01,
6.547670458137801575e+01,
6.569622839567259120e+01,
6.591960545460469234e+01,
6.614680034282939403e+01,
6.637777703970350274e+01,
6.661249892499652958e+01,
6.685092878469657762e+01,
6.709302881691040454e+01,
6.733876063785689325e+01,
6.758808528795242410e+01,
6.784096323798783601e+01,
6.809735439539562663e+01,
6.835721811060626862e+01,
6.862051318349315920e+01,
6.888719786990465366e+01,
6.915722988828235884e+01,
6.943056642636474862e+01,
6.970716414797475124e+01,
6.998697919989049865e+01,
7.026996721879814345e+01,
7.055608333832532253e+01,
7.084528219615452826e+01,
7.113751794121520788e+01,
7.143274424095301356e+01,
7.173091428867579111e+01,
7.203198081097443151e+01,
7.233589607521793141e+01,
7.264261189712104283e+01,
7.295207964838387227e+01,
7.326425026440138311e+01,
7.357907425204261642e+01,
7.389650169749738495e+01,
7.421648227418992860e+01,
7.453896525075806778e+01,
7.486389949909619190e+01,
7.519123350246154303e+01,
7.552091536364190461e+01,
7.585289281318358690e+01,
7.618711321767857214e+01,
7.652352358810929900e+01,
7.686207058824967930e+01,
7.720270054312145191e+01,
7.754535944750402621e+01,
7.788999297449672099e+01,
7.823654648413207724e+01,
7.858496503203873829e+01,
7.893519337815257586e+01,
7.928717599547482564e+01,
7.964085707887551280e+01,
7.999618055394115856e+01,
8.035309008586510515e+01,
8.071152908837908058e+01,
8.107144073272465334e+01,
8.143276795666332646e+01,
8.179545347352322437e+01,
8.215943978128177605e+01,
8.252466917168241878e+01,
8.289108373938378804e+01,
8.325862539114051231e+01,
8.362723585501349532e+01,
8.399685668960863438e+01,
8.436742929334259600e+01,
8.473889491373356009e+01,
8.511119465671630735e+01,
8.548426949597958924e+01,
8.585806028232434528e+01,
8.623250775304161664e+01,
8.660755254130837955e+01,
8.698313518559976387e+01,
8.735919613911642045e+01,
8.773567577922553085e+01,
8.811251441691338471e+01,
8.848965230624911271e+01,
8.886702965385686070e+01,
8.924458662839582246e+01,
8.962226337004628363e+01,
8.999999999999998579e+01
};

static float yaw_sin[500] = {
9.000000000000000000e+01,
9.037774661165408929e+01,
9.075549322330816437e+01,
9.113323983496225367e+01,
9.151098644661632875e+01,
9.188873305827041804e+01,
9.226647966992450733e+01,
9.264422628157858242e+01,
9.302197289323267171e+01,
9.339971950488674679e+01,
9.377746611654083608e+01,
9.415521272819491116e+01,
9.453295933984900046e+01,
9.491070595150307554e+01,
9.528845256315716483e+01,
9.566619917481125412e+01,
9.604394578646532921e+01,
9.642169239811941850e+01,
9.679943900977349358e+01,
9.717718562142758287e+01,
9.755493223308167217e+01,
9.793267884473574725e+01,
9.831042545638983654e+01,
9.868817206804391162e+01,
9.906591867969800091e+01,
9.944366529135209021e+01,
9.982141190300616529e+01,
1.001991585146602546e+02,
1.005769051263143297e+02,
1.009546517379684190e+02,
1.013323983496224940e+02,
1.017101449612765833e+02,
1.020878915729306584e+02,
1.024656381845847477e+02,
1.028433847962388370e+02,
1.032211314078929121e+02,
1.035988780195470014e+02,
1.039766246312010765e+02,
1.043543712428551657e+02,
1.047321178545092550e+02,
1.051098644661633301e+02,
1.054876110778174194e+02,
1.058653576894714945e+02,
1.062431043011255838e+02,
1.066208509127796731e+02,
1.069985975244337482e+02,
1.073763441360878232e+02,
1.077540907477419125e+02,
1.081318373593960018e+02,
1.085095839710500911e+02,
1.088873305827041662e+02,
1.092650771943582413e+02,
1.096428238060123306e+02,
1.100205704176664199e+02,
1.103983170293205092e+02,
1.107760636409745842e+02,
1.111538102526286593e+02,
1.115315568642827486e+02,
1.119093034759368379e+02,
1.122870500875909130e+02,
1.126647966992450023e+02,
1.130425433108990774e+02,
1.134202899225531667e+02,
1.137980365342072560e+02,
1.141757831458613310e+02,
1.145535297575154203e+02,
1.149312763691694954e+02,
1.153090229808235847e+02,
1.156867695924776740e+02,
1.160645162041317491e+02,
1.164422628157858242e+02,
1.168200094274399135e+02,
1.171977560390940027e+02,
1.175755026507480778e+02,
1.179532492624021671e+02,
1.183309958740562422e+02,
1.187087424857103315e+02,
1.190864890973644208e+02,
1.194642357090184959e+02,
1.198419823206725852e+02,
1.202197289323266602e+02,
1.205974755439807495e+02,
1.209752221556348388e+02,
1.213529687672889139e+02,
1.217307153789430032e+02,
1.221084619905970783e+02,
1.224862086022511676e+02,
1.228639552139052569e+02,
1.232417018255593320e+02,
1.236194484372134070e+02,
1.239971950488674963e+02,
1.243749416605215856e+02,
1.247526882721756607e+02,
1.251304348838297500e+02,
1.255081814954838251e+02,
1.258859281071379144e+02,
1.262636747187920037e+02,
1.266414213304460787e+02,
1.270191679421001680e+02,
1.273969145537542431e+02,
1.277746611654083324e+02,
1.281524077770624217e+02,
1.285301543887164826e+02,
1.289079010003706003e+02,
1.292856476120246612e+02,
1.296633942236787504e+02,
1.300411408353328397e+02,
1.304188874469869006e+02,
1.307966340586410183e+02,
1.311743806702950792e+02,
1.315521272819491685e+02,
1.319298738936032578e+02,
1.323076205052573187e+02,
1.326853671169114079e+02,
1.330631137285654972e+02,
1.334408603402195865e+02,
1.338186069518736758e+02,
1.341963535635277367e+02,
1.345741001751818260e+02,
1.349518467868359153e+02,
1.353295933984900046e+02,
1.357073400101440939e+02,
1.360850866217981547e+02,
1.364628332334522440e+02,
1.368405798451063333e+02,
1.372183264567604226e+02,
1.375960730684145119e+02,
1.379738196800685728e+02,
1.383515662917226621e+02,
1.387293129033767514e+02,
1.391070595150308407e+02,
1.394848061266849299e+02,
1.398625527383389908e+02,
1.402402993499930801e+02,
1.406180459616471694e+02,
1.409957925733012587e+02,
1.413735391849553480e+02,
1.417512857966094089e+02,
1.421290324082634982e+02,
1.425067790199175874e+02,
1.428845256315716483e+02,
1.432622722432257376e+02,
1.436400188548798269e+02,
1.440177654665339162e+02,
1.443955120781880055e+02,
1.447732586898420664e+02,
1.451510053014961557e+02,
1.455287519131502449e+02,
1.459064985248043342e+02,
1.462842451364584235e+02,
1.466619917481124844e+02,
1.470397383597665737e+02,
1.474174849714206630e+02,
1.477952315830747523e+02,
1.481729781947288416e+02,
1.485507248063829024e+02,
1.489284714180369917e+02,
1.493062180296910810e+02,
1.496839646413451703e+02,
1.500617112529992596e+02,
1.504394578646533205e+02,
1.508172044763074098e+02,
1.511949510879614991e+02,
1.515726976996155599e+02,
1.519504443112696777e+02,
1.523281909229237385e+02,
1.527059375345778278e+02,
1.530836841462319171e+02,
1.534614307578860064e+02,
1.538391773695400957e+02,
1.542169239811941566e+02,
1.545946705928482459e+02,
1.549724172045023352e+02,
1.553501638161563960e+02,
1.557279104278105137e+02,
1.561056570394645746e+02,
1.564834036511186639e+02,
1.568611502627727532e+02,
1.572388968744268141e+02,
1.576166434860809318e+02,
1.579943900977349927e+02,
1.583721367093890819e+02,
1.587498833210431712e+02,
1.591276299326972321e+02,
1.595053765443513214e+02,
1.598831231560054107e+02,
1.602608697676595000e+02,
1.606386163793135893e+02,
1.610163629909676501e+02,
1.613941096026217679e+02,
1.617718562142758287e+02,
1.621496028259298896e+02,
1.625273494375840073e+02,
1.629050960492380682e+02,
1.632828426608921575e+02,
1.636605892725462468e+02,
1.640383358842003361e+02,
1.644160824958544254e+02,
1.647938291075084862e+02,
1.651715757191626039e+02,
1.655493223308166648e+02,
1.659270689424707257e+02,
1.663048155541248434e+02,
1.666825621657789043e+02,
1.670603087774329936e+02,
1.674380553890870829e+02,
1.678158020007411722e+02,
1.681935486123952614e+02,
1.685712952240493223e+02,
1.689490418357034400e+02,
1.693267884473575009e+02,
1.697045350590115618e+02,
1.700822816706656511e+02,
1.704600282823197404e+02,
1.708377748939738296e+02,
1.712155215056279189e+02,
1.715932681172820082e+02,
1.719710147289360975e+02,
1.723487613405901584e+02,
1.727265079522442477e+02,
1.731042545638983370e+02,
1.734820011755523979e+02,
1.738597477872064871e+02,
1.742374943988605764e+02,
1.746152410105146657e+02,
1.749929876221687550e+02,
1.753707342338228159e+02,
1.757484808454769336e+02,
1.761262274571309945e+02,
1.765039740687850838e+02,
1.768817206804391731e+02,
1.772594672920932339e+02,
1.776372139037473232e+02,
1.780149605154014125e+02,
1.783927071270555018e+02,
1.787704537387095911e+02,
1.791482003503636520e+02,
1.795259469620177697e+02,
1.799036935736718306e+02,
1.802814401853258914e+02,
1.806591867969800091e+02,
1.810369334086340700e+02,
1.814146800202881593e+02,
1.817924266319422486e+02,
1.821701732435963379e+02,
1.825479198552504272e+02,
1.829256664669044881e+02,
1.833034130785585774e+02,
1.836811596902126666e+02,
1.840589063018667275e+02,
1.844366529135208452e+02,
1.848143995251749061e+02,
1.851921461368289954e+02,
1.855698927484830847e+02,
1.859476393601371456e+02,
1.863253859717912633e+02,
1.867031325834453241e+02,
1.870808791950994134e+02,
1.874586258067535027e+02,
1.878363724184075636e+02,
1.882141190300616813e+02,
1.885918656417157422e+02,
1.889696122533698315e+02,
1.893473588650239208e+02,
1.897251054766779816e+02,
1.901028520883320994e+02,
1.904805986999861602e+02,
1.908583453116402495e+02,
1.912360919232943388e+02,
1.916138385349483997e+02,
1.919915851466025174e+02,
1.923693317582565783e+02,
1.927470783699106676e+02,
1.931248249815647569e+02,
1.935025715932188177e+02,
1.938803182048729070e+02,
1.942580648165269963e+02,
1.946358114281810856e+02,
1.950135580398351749e+02,
1.953913046514892358e+02,
1.957690512631433251e+02,
1.961467978747974144e+02,
1.965245444864514752e+02,
1.969022910981055929e+02,
1.972800377097596538e+02,
1.976577843214137431e+02,
1.980355309330678324e+02,
1.984132775447219217e+02,
1.987910241563760110e+02,
1.991687707680300718e+02,
1.995465173796841611e+02,
1.999242639913382504e+02,
2.003020106029923113e+02,
2.006797572146464290e+02,
2.010575038263004899e+02,
2.014352504379545792e+02,
2.018129970496086685e+02,
2.021907436612627578e+02,
2.025684902729168471e+02,
2.029462368845709079e+02,
2.033239834962249972e+02,
2.037017301078790865e+02,
2.040794767195331474e+02,
2.044572233311872367e+02,
2.048349699428413260e+02,
2.052127165544954153e+02,
2.055904631661495046e+02,
2.059682097778035654e+02,
2.063459563894576831e+02,
2.067237030011117440e+02,
2.071014496127658049e+02,
2.074791962244199226e+02,
2.078569428360739835e+02,
2.082346894477280728e+02,
2.086124360593821621e+02,
2.089901826710362513e+02,
2.093679292826903406e+02,
2.097456758943444015e+02,
2.101234225059984908e+02,
2.105011691176525801e+02,
2.108789157293066694e+02,
2.112566623409607587e+02,
2.116344089526148196e+02,
2.120121555642689373e+02,
2.123899021759229981e+02,
2.127676487875770874e+02,
2.131453953992311483e+02,
2.135231420108852376e+02,
2.139008886225393553e+02,
2.142786352341934162e+02,
2.146563818458474771e+02,
2.150341284575015948e+02,
2.154118750691556556e+02,
2.157896216808097449e+02,
2.161673682924638342e+02,
2.165451149041178951e+02,
2.169228615157720128e+02,
2.173006081274260737e+02,
2.176783547390801630e+02,
2.180561013507342523e+02,
2.184338479623883131e+02,
2.188115945740424024e+02,
2.191893411856964917e+02,
2.195670877973505810e+02,
2.199448344090046703e+02,
2.203225810206587596e+02,
2.207003276323128205e+02,
2.210780742439669098e+02,
2.214558208556209991e+02,
2.218335674672750599e+02,
2.222113140789291776e+02,
2.225890606905832669e+02,
2.229668073022373278e+02,
2.233445539138914171e+02,
2.237223005255454780e+02,
2.241000471371995673e+02,
2.244777937488536566e+02,
2.248555403605077458e+02,
2.252332869721618351e+02,
2.256110335838159244e+02,
2.259887801954699853e+02,
2.263665268071240746e+02,
2.267442734187781639e+02,
2.271220200304322248e+02,
2.274997666420863425e+02,
2.278775132537404318e+02,
2.282552598653944926e+02,
2.286330064770485819e+02,
2.290107530887026428e+02,
2.293884997003567321e+02,
2.297662463120108214e+02,
2.301439929236649107e+02,
2.305217395353190000e+02,
2.308994861469730893e+02,
2.312772327586271501e+02,
2.316549793702812394e+02,
2.320327259819353287e+02,
2.324104725935894180e+02,
2.327882192052435073e+02,
2.331659658168975966e+02,
2.335437124285516575e+02,
2.339214590402057468e+02,
2.342992056518598076e+02,
2.346769522635138969e+02,
2.350546988751680146e+02,
2.354324454868220755e+02,
2.358101920984761648e+02,
2.361879387101302541e+02,
2.365656853217843150e+02,
2.369434319334384043e+02,
2.373211785450924936e+02,
2.376989251567465828e+02,
2.380766717684006721e+02,
2.384544183800547614e+02,
2.388321649917088223e+02,
2.392099116033629116e+02,
2.395876582150169725e+02,
2.399654048266710902e+02,
2.403431514383251795e+02,
2.407208980499792403e+02,
2.410986446616333296e+02,
2.414763912732874189e+02,
2.418541378849414798e+02,
2.422318844965955691e+02,
2.426096311082496868e+02,
2.429873777199037477e+02,
2.433651243315578370e+02,
2.437428709432119263e+02,
2.441206175548659871e+02,
2.444983641665200764e+02,
2.448761107781741373e+02,
2.452538573898282550e+02,
2.456316040014823443e+02,
2.460093506131364052e+02,
2.463870972247904945e+02,
2.467648438364445838e+02,
2.471425904480986446e+02,
2.475203370597527339e+02,
2.478980836714068516e+02,
2.482758302830609125e+02,
2.486535768947150018e+02,
2.490313235063690911e+02,
2.494090701180231520e+02,
2.497868167296772413e+02,
2.501645633413313021e+02,
2.505423099529854198e+02,
2.509200565646395091e+02,
2.512978031762935700e+02,
2.516755497879476593e+02,
2.520532963996017486e+02,
2.524310430112558095e+02,
2.528087896229098988e+02,
2.531865362345640165e+02,
2.535642828462180773e+02,
2.539420294578721666e+02,
2.543197760695262559e+02,
2.546975226811803168e+02,
2.550752692928344061e+02,
2.554530159044884954e+02,
2.558307625161425847e+02,
2.562085091277966740e+02,
2.565862557394507348e+02,
2.569640023511047957e+02,
2.573417489627589134e+02,
2.577194955744129743e+02,
2.580972421860670920e+02,
2.584749887977211529e+02,
2.588527354093752137e+02,
2.592304820210293315e+02,
2.596082286326834492e+02,
2.599859752443375100e+02,
2.603637218559915709e+02,
2.607414684676456318e+02,
2.611192150792997495e+02,
2.614969616909538672e+02,
2.618747083026079281e+02,
2.622524549142619890e+02,
2.626302015259160498e+02,
2.630079481375701675e+02,
2.633856947492242853e+02,
2.637634413608783461e+02,
2.641411879725324070e+02,
2.645189345841864679e+02,
2.648966811958405856e+02,
2.652744278074946465e+02,
2.656521744191487642e+02,
2.660299210308028250e+02,
2.664076676424568859e+02,
2.667854142541110036e+02,
2.671631608657650645e+02,
2.675409074774191822e+02,
2.679186540890732431e+02,
2.682964007007273040e+02,
2.686741473123814217e+02,
2.690518939240355394e+02,
2.694296405356896003e+02,
2.698073871473436611e+02,
2.701851337589977220e+02,
2.705628803706517829e+02,
2.709406269823059574e+02,
2.713183735939600183e+02,
2.716961202056140792e+02,
2.720738668172681400e+02,
2.724516134289222009e+02,
2.728293600405763186e+02,
2.732071066522304363e+02,
2.735848532638844972e+02,
2.739625998755385581e+02,
2.743403464871926758e+02,
2.747180930988467367e+02,
2.750958397105008544e+02,
2.754735863221549153e+02,
2.758513329338089761e+02,
2.762290795454630938e+02,
2.766068261571171547e+02,
2.769845727687712724e+02,
2.773623193804253333e+02,
2.777400659920793942e+02,
2.781178126037334550e+02,
2.784955592153875727e+02
};

static float yaw_circle[500] = {
9.000000000000000000e+01,
9.037773662995370216e+01,
9.075541337160414912e+01,
9.113297034614312508e+01,
9.151034769375085887e+01,
9.188748558308658687e+01,
9.226432422077446915e+01,
9.264080386088355112e+01,
9.301686481440023613e+01,
9.339244745869160624e+01,
9.376749224695835494e+01,
9.414193971767564051e+01,
9.451573050402039655e+01,
9.488880534328367844e+01,
9.526110508626642570e+01,
9.563257070665738979e+01,
9.600314331039133720e+01,
9.637276414498649046e+01,
9.674137460885945927e+01,
9.710891626061618354e+01,
9.747533082831756701e+01,
9.784056021871819553e+01,
9.820454652647677563e+01,
9.856723204333665933e+01,
9.892855926727531823e+01,
9.928847091162091942e+01,
9.964690991413488064e+01,
1.000038194460588272e+02,
1.003591429211244730e+02,
1.007128240045251601e+02,
1.010648066218473957e+02,
1.014150349679612475e+02,
1.017634535158678943e+02,
1.021100070255032648e+02,
1.024546405524959596e+02,
1.027972994568785339e+02,
1.031379294117503207e+02,
1.034764764118907010e+02,
1.038128867823213994e+02,
1.041471071868163989e+02,
1.044790846363580812e+02,
1.048087664975384428e+02,
1.051361005009037939e+02,
1.054610347492419180e+02,
1.057835177258100430e+02,
1.061034983025026008e+02,
1.064209257479573694e+02,
1.067357497355985885e+02,
1.070479203516161135e+02,
1.073573881028789430e+02,
1.076641039247820686e+02,
1.079680191890255543e+02,
1.082690857113242089e+02,
1.085672557590469722e+02,
1.088624820587847921e+02,
1.091547178038454575e+02,
1.094439166616746775e+02,
1.097300327812018566e+02,
1.100130208001094871e+02,
1.102928358520252488e+02,
1.105694335736352230e+02,
1.108427701117176269e+02,
1.111128021300953463e+02,
1.113794868165068408e+02,
1.116427818893937314e+02,
1.119026456046043734e+02,
1.121590367620121356e+02,
1.124119147120475759e+02,
1.126612393621431067e+02,
1.129069711830895812e+02,
1.131490712153034224e+02,
1.133875010750034562e+02,
1.136222229602964831e+02,
1.138531996571706060e+02,
1.140803945453953077e+02,
1.143037716043274088e+02,
1.145232954186219700e+02,
1.147389311838472565e+02,
1.149506447120027417e+02,
1.151584024369395109e+02,
1.153621714196819710e+02,
1.155619193536501967e+02,
1.157576145697819499e+02,
1.159492260415536720e+02,
1.161367233898996005e+02,
1.163200768880281828e+02,
1.164992574661351625e+02,
1.166742367160124445e+02,
1.168449868955520117e+02,
1.170114809331443979e+02,
1.171736924319707072e+02,
1.173315956741877244e+02,
1.174851656250052940e+02,
1.176343779366555822e+02,
1.177792089522531569e+02,
1.179196357095457870e+02,
1.180556359445549077e+02,
1.181871880951055545e+02,
1.183142713042448406e+02,
1.184368654235488378e+02,
1.185549510163168918e+02,
1.186685093606533030e+02,
1.187775224524355764e+02,
1.188819730081688562e+02,
1.189818444677261624e+02,
1.190771209969739317e+02,
1.191677874902824072e+02,
1.192538295729205799e+02,
1.193352336033352543e+02,
1.194119866753138126e+02,
1.194840766200304358e+02,
1.195514920079754404e+02,
1.196142221507673753e+02,
1.196722571028475670e+02,
1.197255876630569844e+02,
1.197742053760949688e+02,
1.198181025338598999e+02,
1.198572721766711595e+02,
1.198917080943726319e+02,
1.199214048273172608e+02,
1.199463576672326326e+02,
1.199665626579675148e+02,
1.199820165961189957e+02,
1.199927170315404368e+02,
1.199986622677299124e+02,
1.199998513620991787e+02,
1.199962841261231148e+02,
1.199879611253696226e+02,
1.199748836794099134e+02,
1.199570538616093955e+02,
1.199344744987988349e+02,
1.199071491708262585e+02,
1.198750822099893583e+02,
1.198382787003485959e+02,
1.197967444769212335e+02,
1.197504861247561223e+02,
1.196995109778897586e+02,
1.196438271181834239e+02,
1.195834433740419342e+02,
1.195183693190138570e+02,
1.194486152702737485e+02,
1.193741922869863430e+02,
1.192951121685532456e+02,
1.192113874527421302e+02,
1.191230314136989819e+02,
1.190300580598435403e+02,
1.189324821316483138e+02,
1.188303190993015335e+02,
1.187235851602545154e+02,
1.186122972366535180e+02,
1.184964729726569033e+02,
1.183761307316377156e+02,
1.182512895932722472e+02,
1.181219693505150445e+02,
1.179881905064608532e+02,
1.178499742710939557e+02,
1.177073425579253865e+02,
1.175603179805187466e+02,
1.174089238489047773e+02,
1.172531841658858696e+02,
1.170931236232303121e+02,
1.169287675977577550e+02,
1.167601421473156620e+02,
1.165872740066480588e+02,
1.164101905831568473e+02,
1.162289199525564811e+02,
1.160434908544227426e+02,
1.158539326876361599e+02,
1.156602755057210032e+02,
1.154625500120804134e+02,
1.152607875551285730e+02,
1.150550201233205030e+02,
1.148452803400805351e+02,
1.146316014586299161e+02,
1.144140173567147798e+02,
1.141925625312349410e+02,
1.139672720927745502e+02,
1.137381817600355731e+02,
1.135053278541746522e+02,
1.132687472930446830e+02,
1.130284775853415482e+02,
1.127845568246573862e+02,
1.125370236834409923e+02,
1.122859174068665311e+02,
1.120312778066113424e+02,
1.117731452545440618e+02,
1.115115606763237963e+02,
1.112465655449116042e+02,
1.109782018739951326e+02,
1.107065122113276061e+02,
1.104315396319819911e+02,
1.101533277315217276e+02,
1.098719206190888542e+02,
1.095873629104106897e+02,
1.092996997207262666e+02,
1.090089766576334682e+02,
1.087152398138582754e+02,
1.084185357599468489e+02,
1.081189115368821234e+02,
1.078164146486255675e+02,
1.075110930545858139e+02,
1.072029951620147585e+02,
1.068921698183329596e+02,
1.065786663033850630e+02,
1.062625343216267026e+02,
1.059438239942441555e+02,
1.056225858512078162e+02,
1.052988708232609554e+02,
1.049727302338448851e+02,
1.046442157909618658e+02,
1.043133795789770772e+02,
1.039802740503608902e+02,
1.036449520173727592e+02,
1.033074666436881159e+02,
1.029678714359695419e+02,
1.026262202353834994e+02,
1.022825672090641405e+02,
1.019369668415253187e+02,
1.015894739260223787e+02,
1.012401435558649041e+02,
1.008890311156819450e+02,
1.005361922726411450e+02,
1.001816829676228622e+02,
9.982555940635106140e+01,
9.946787805048222708e+01,
9.910869560865351957e+01,
9.874806902749203630e+01,
9.838605548258607314e+01,
9.802271236942033283e+01,
9.765809729427606101e+01,
9.729226806509784353e+01,
9.692528268232855737e+01,
9.655719932971354069e+01,
9.618807636507591496e+01,
9.581797231106423851e+01,
9.544694584587401209e+01,
9.507505579394452866e+01,
9.470236111663254519e+01,
9.432892090286426878e+01,
9.395479435976709226e+01,
9.358004080328254304e+01,
9.320471964876209370e+01,
9.282889040154714166e+01,
9.245261264753466435e+01,
9.207594604373022662e+01,
9.169895030878964803e+01,
9.132168521355087876e+01,
9.094421057155760479e+01,
9.056658622957615989e+01,
9.018887205810707997e+01,
8.981112794189293425e+01,
8.943341377042385432e+01,
8.905578942844242363e+01,
8.867831478644914966e+01,
8.830104969121036618e+01,
8.792405395626978759e+01,
8.754738735246534986e+01,
8.717110959845288676e+01,
8.679528035123790630e+01,
8.641995919671747117e+01,
8.604520564023293616e+01,
8.567107909713574543e+01,
8.529763888336746902e+01,
8.492494420605548555e+01,
8.455305415412600212e+01,
8.418202768893576149e+01,
8.381192363492409925e+01,
8.344280067028648773e+01,
8.307471731767147105e+01,
8.270773193490217068e+01,
8.234190270572396741e+01,
8.197728763057968138e+01,
8.161394451741392686e+01,
8.125193097250797791e+01,
8.089130439134649464e+01,
8.053212194951780134e+01,
8.017444059364893860e+01,
7.981831703237715203e+01,
7.946380772735886922e+01,
7.911096888431806917e+01,
7.875985644413512432e+01,
7.841052607397763552e+01,
7.806303315847469548e+01,
7.771743279093587375e+01,
7.737377976461651485e+01,
7.703212856403048647e+01,
7.669253335631188406e+01,
7.635504798262725501e+01,
7.601972594963912400e+01,
7.568662042102293697e+01,
7.535578420903814845e+01,
7.502726976615512910e+01,
7.470112917673905883e+01,
7.437741414879219803e+01,
7.405617600575585868e+01,
7.373746567837331156e+01,
7.342133369661495124e+01,
7.310783018166705460e+01,
7.279700483798525568e+01,
7.248890694541421453e+01,
7.218358535137443255e+01,
7.188108846311789080e+01,
7.158146424005316533e+01,
7.128476018614173881e+01,
7.099102334236653178e+01,
7.070030027927376182e+01,
7.041263708958932455e+01,
7.012807938091116000e+01,
6.984667226847828658e+01,
6.956846036801802313e+01,
6.929348778867240810e+01,
6.902179812600486741e+01,
6.875343445508841000e+01,
6.848843932367623211e+01,
6.822685474545595241e+01,
6.796872219338865762e+01,
6.771408259313348310e+01,
6.746297631655900773e+01,
6.721544317534262802e+01,
6.697152241465846600e+01,
6.673125270695531697e+01,
6.649467214582534780e+01,
6.626181823996444109e+01,
6.603272790722544983e+01,
6.580743746876507316e+01,
6.558598264328522021e+01,
6.536839854137009809e+01,
6.515471965991947911e+01,
6.494497987667949701e+01,
6.473921244487144122e+01,
6.453744998791960086e+01,
6.433972449427899676e+01,
6.414606731236385428e+01,
6.395650914557725741e+01,
6.377108004744351888e+01,
6.358980941684316690e+01,
6.341272599335194826e+01,
6.323985785268433801e+01,
6.307123240224225924e+01,
6.290687637676968791e+01,
6.274681583411414465e+01,
6.259107615109522271e+01,
6.243968201948126051e+01,
6.229265744207461353e+01,
6.215002572890605848e+01,
6.201180949353914684e+01,
6.187803064948496967e+01,
6.174871040672775990e+01,
6.162386926836227730e+01,
6.150352702734309673e+01,
6.138770276334648202e+01,
6.127641483974548464e+01,
6.116968090069846653e+01,
6.106751786835169327e+01,
6.096994194015645974e+01,
6.087696858630101815e+01,
6.078861254725786978e+01,
6.070488783144676148e+01,
6.062580771301365701e+01,
6.055138472972625152e+01,
6.048163068098614303e+01,
6.041655662595806575e+01,
6.035617288181657614e+01,
6.030048902211024142e+01,
6.024951387524387059e+01,
6.020325552307876649e+01,
6.016172129965140414e+01,
6.012491779001065595e+01,
6.009285082917374154e+01,
6.006552550120116507e+01,
6.004294613839060446e+01,
6.002511632059007951e+01,
6.001203887463037745e+01,
6.000371587387688521e+01,
6.000014863790082131e+01,
6.000133773227008760e+01,
6.000728296845956322e+01,
6.001798340388100428e+01,
6.003343734203248516e+01,
6.005364233276736030e+01,
6.007859517268273919e+01,
6.010829190562736102e+01,
6.014272782332883338e+01,
6.018189746614010005e+01,
6.022579462390503124e+01,
6.027441233694301559e+01,
6.032774289715241878e+01,
6.038577784923261760e+01,
6.044850799202454539e+01,
6.051592337996956417e+01,
6.058801332468619449e+01,
6.066476639666474568e+01,
6.074617042707940584e+01,
6.083221250971759275e+01,
6.092287900302605408e+01,
6.101815553227383049e+01,
6.111802699183113674e+01,
6.122247754756442362e+01,
6.133149063934669698e+01,
6.144504898368310819e+01,
6.156313457645116216e+01,
6.168572869575515227e+01,
6.181281190489443844e+01,
6.194436405544507807e+01,
6.208036429045421301e+01,
6.222079104774682889e+01,
6.236562206334441072e+01,
6.251483437499469886e+01,
6.266840432581227560e+01,
6.282630756802928573e+01,
6.298851906685558788e+01,
6.315501310444798833e+01,
6.332576328398755550e+01,
6.350074253386481615e+01,
6.367992311197180300e+01,
6.386327661010039947e+01,
6.405077395844631383e+01,
6.424238543021805015e+01,
6.443808064634980326e+01,
6.463782858031801482e+01,
6.484159756306047484e+01,
6.504935528799725830e+01,
6.526106881615274347e+01,
6.547670458137801575e+01,
6.569622839567259120e+01,
6.591960545460469234e+01,
6.614680034282939403e+01,
6.637777703970350274e+01,
6.661249892499652958e+01,
6.685092878469657762e+01,
6.709302881691040454e+01,
6.733876063785689325e+01,
6.758808528795242410e+01,
6.784096323798783601e+01,
6.809735439539562663e+01,
6.835721811060626862e+01,
6.862051318349315920e+01,
6.888719786990465366e+01,
6.915722988828235884e+01,
6.943056642636474862e+01,
6.970716414797475124e+01,
6.998697919989049865e+01,
7.026996721879814345e+01,
7.055608333832532253e+01,
7.084528219615452826e+01,
7.113751794121520788e+01,
7.143274424095301356e+01,
7.173091428867579111e+01,
7.203198081097443151e+01,
7.233589607521793141e+01,
7.264261189712104283e+01,
7.295207964838387227e+01,
7.326425026440138311e+01,
7.357907425204261642e+01,
7.389650169749738495e+01,
7.421648227418992860e+01,
7.453896525075806778e+01,
7.486389949909619190e+01,
7.519123350246154303e+01,
7.552091536364190461e+01,
7.585289281318358690e+01,
7.618711321767857214e+01,
7.652352358810929900e+01,
7.686207058824967930e+01,
7.720270054312145191e+01,
7.754535944750402621e+01,
7.788999297449672099e+01,
7.823654648413207724e+01,
7.858496503203873829e+01,
7.893519337815257586e+01,
7.928717599547482564e+01,
7.964085707887551280e+01,
7.999618055394115856e+01,
8.035309008586510515e+01,
8.071152908837908058e+01,
8.107144073272465334e+01,
8.143276795666332646e+01,
8.179545347352322437e+01,
8.215943978128177605e+01,
8.252466917168241878e+01,
8.289108373938378804e+01,
8.325862539114051231e+01,
8.362723585501349532e+01,
8.399685668960863438e+01,
8.436742929334259600e+01,
8.473889491373356009e+01,
8.511119465671630735e+01,
8.548426949597958924e+01,
8.585806028232434528e+01,
8.623250775304161664e+01,
8.660755254130837955e+01,
8.698313518559976387e+01,
8.735919613911642045e+01,
8.773567577922553085e+01,
8.811251441691338471e+01,
8.848965230624911271e+01,
8.886702965385686070e+01,
8.924458662839582246e+01,
8.962226337004628363e+01,
8.999999999999998579e+01
};

static float pitch_circle[500] = {
1.200000000000000000e+02
,1.199976218230444260e+02
,1.199904876692260842e+02
,1.199785986696303155e+02
,1.199619567092001091e+02
,1.199405644264372626e+02
,1.199144252129840140e+02
,1.198835432130854031e+02
,1.198479233229321039e+02
,1.198075711898842428e+02
,1.197624932115760146e+02
,1.197126965349013972e+02
,1.196581890548809497e+02
,1.195989794134102056e+02
,1.195350769978895045e+02
,1.194664919397356329e+02
,1.193932351127755851e+02
,1.193153181315225453e+02
,1.192327533493344589e+02
,1.191455538564554786e+02
,1.190537334779405683e+02
,1.189573067714636352e+02
,1.188562890250094028e+02
,1.187506962544496503e+02
,1.186405452010039738e+02
,1.185258533285854980e+02
,1.184066388210320611e+02
,1.182829205792233438e+02
,1.181547182180840991e+02
,1.180220520634743906e+02
,1.178849431489670110e+02
,1.177434132125126638e+02
,1.175974846929935893e+02
,1.174471807266659340e+02
,1.172925251434916305e+02
,1.171335424633602997e+02
,1.169702578922016585e+02
,1.168026973179893560e+02
,1.166308873066364526e+02
,1.164548550977836214e+02
,1.162746286004803267e+02
,1.160902363887601325e+02
,1.159017076971102398e+02
,1.157090724158366726e+02
,1.155123610863251997e+02
,1.153116048961992135e+02
,1.151068356743750343e+02
,1.148980858860156502e+02
,1.146853886273834746e+02
,1.144687776205930732e+02
,1.142482872082647987e+02
,1.140239523480798027e+02
,1.137958086072377455e+02
,1.135638921568177437e+02
,1.133282397660436800e+02
,1.130888887964545830e+02
,1.128458771959811031e+02
,1.125992434929291477e+02
,1.123490267898713029e+02
,1.120952667574473764e+02
,1.118380036280747873e+02
,1.115772781895699381e+02
,1.113131317786814662e+02
,1.110456062745365102e+02
,1.107747440920010149e+02
,1.105005881749549985e+02
,1.102231819894841180e+02
,1.099425695169881720e+02
,1.096587952472082037e+02
,1.093719041711728295e+02
,1.090819417740650863e+02
,1.087889540280110339e+02
,1.084929873847910642e+02
,1.081940887684752113e+02
,1.078923055679835841e+02
,1.075876856295730164e+02
,1.072802772492513839e+02
,1.069701291651203974e+02
,1.066572905496485077e+02
,1.063418110018748024e+02
,1.060237405395452868e+02
,1.057031295911828863e+02
,1.053800289880922065e+02
,1.050544899563005004e+02
,1.047265641084359942e+02
,1.043963034355449935e+02
,1.040637602988489334e+02
,1.037289874214427670e+02
,1.033920378799360122e+02
,1.030529650960376387e+02
,1.027118228280863832e+02
,1.023686651625275772e+02
,1.020235465053380608e+02
,1.016765215734003220e+02
,1.013276453858274522e+02
,1.009769732552400825e+02
,1.006245607789968659e+02
,1.002704638303798106e+02
,9.991473854973578739e+01
,9.955744133557580255e+01
,9.919862883563324374e+01
,9.883835793788269086e+01
,9.847668576152062769e+01
,9.811366964790936152e+01
,9.774936715148599831e+01
,9.738383603063729765e+01
,9.701713423854246798e+01
,9.664931991398496791e+01
,9.628045137213480587e+01
,9.591058709530307169e+01
,9.553978572366973765e+01
,9.516810604598660461e+01
,9.479560699025664405e+01
,9.442234761439121371e+01
,9.404838709684669595e+01
,9.367378472724210781e+01
,9.329859989695900424e+01
,9.292289208972516690e+01
,9.254672087218392562e+01
,9.217014588444996548e+01
,9.179322683065385036e+01
,9.141602346947607316e+01
,9.103859560467270740e+01
,9.066100307559381122e+01
,9.028330574769610450e+01
,8.990556350305170952e+01
,8.952783623085403519e+01
,8.915018381792269508e+01
,8.877266613920872373e+01
,8.839534304830172573e+01
,8.801827436794033588e+01
,8.764151988052763897e+01
,8.726513931865297025e+01
,8.688919235562157439e+01
,8.651373859599371485e+01
,8.613883756613465437e+01
,8.576454870477705583e+01
,8.539093135359725295e+01
,8.501804474780688281e+01
,8.464594800676148623e+01
,8.427470012458735482e+01
,8.390435996082840120e+01
,8.353498623111414645e+01
,8.316663749785072923e+01
,8.279937216093610175e+01
,8.243324844850097577e+01
,8.206832440767706771e+01
,8.170465789539402124e+01
,8.134230656920641422e+01
,8.098132787815251277e+01
,8.062177905364593755e+01
,8.026371710040197627e+01
,7.990719878739965054e+01
,7.955228063888139900e+01
,7.919901892539132859e+01
,7.884746965485388159e+01
,7.849768856369398407e+01
,7.814973110800032430e+01
,7.780365245473308278e+01
,7.745950747297746375e+01
,7.711735072524446366e+01
,7.677723645882026915e+01
,7.643921859716557776e+01
,7.610335073136633355e+01
,7.576968611163711387e+01
,7.543827763887856008e+01
,7.510917785629018795e+01
,7.478243894103991352e+01
,7.445811269599168725e+01
,7.413625054149228788e+01
,7.381690350721896721e+01
,7.350012222408891205e+01
,7.318595691623195876e+01
,7.287445739302779657e+01
,7.256567304120892459e+01
,7.225965281703068399e+01
,7.195644523850940288e+01
,7.165609837773016011e+01
,7.135865985322512017e+01
,7.106417682242397404e+01
,7.077269597417719638e+01
,7.048426352135388129e+01
,7.019892519351488147e+01
,6.991672622966257222e+01
,6.963771137106847675e+01
,6.936192485417970488e+01
,6.908941040360554098e+01
,6.882021122518509060e+01
,6.855436999913719376e+01
,6.829192887329367068e+01
,6.803292945641706524e+01
,6.777741281160365361e+01
,6.752541944977322430e+01
,6.727698932324618397e+01
,6.703216181940932472e+01
,6.679097575447114821e+01
,6.655346936730778395e+01
,6.631968031340034031e+01
,6.608964565886485332e+01
,6.586340187457560091e+01
,6.564098483038289089e+01
,6.542242978942596210e+01
,6.520777140254226367e+01
,6.499704370277376597e+01
,6.479028009997109905e+01
,6.458751337549659866e+01
,6.438877567702704141e+01
,6.419409851345676543e+01
,6.400351274990201489e+01
,6.381704860280753167e+01
,6.363473563515583464e+01
,6.345660275178013876e+01
,6.328267819478166700e+01
,6.311298953905198061e+01
,6.294756368790113044e+01
,6.278642686879225465e+01
,6.262960462918333349e+01
,6.247712183247680429e+01
,6.232900265407754858e+01
,6.218527057756001142e+01
,6.204594839094500003e+01
,6.191105818308676589e+01
,6.178062134017089591e+01
,6.165465854232364507e+01
,6.153318976033322940e+01
,6.141623425248351964e+01
,6.130381056150077512e+01
,6.119593651161370929e+01
,6.109262920572765410e+01
,6.099390502271286607e+01
,6.089977961480782653e+01
,6.081026790513760005e+01
,6.072538408534785503e+01
,6.064514161335488751e+01
,6.056955321121186842e+01
,6.049863086309186144e+01
,6.043238581338781756e+01
,6.037082856492975935e+01
,6.031396887731968093e+01
,6.026181576538415641e+01
,6.021437749774511872e+01
,6.017166159550890825e+01
,6.013367483107380451e+01
,6.010042322705632500e+01
,6.007191205533636236e+01
,6.004814583622136581e+01
,6.002912833772963097e+01
,6.001486257499293941e+01
,6.000535080977850555e+01
,6.000059455013038701e+01
,6.000059455013038701e+01
,6.000535080977850555e+01
,6.001486257499293941e+01
,6.002912833772963097e+01
,6.004814583622136581e+01
,6.007191205533636236e+01
,6.010042322705632500e+01
,6.013367483107380451e+01
,6.017166159550890825e+01
,6.021437749774511872e+01
,6.026181576538415641e+01
,6.031396887731967382e+01
,6.037082856492975225e+01
,6.043238581338781046e+01
,6.049863086309186144e+01
,6.056955321121186842e+01
,6.064514161335488041e+01
,6.072538408534785503e+01
,6.081026790513759295e+01
,6.089977961480782653e+01
,6.099390502271286607e+01
,6.109262920572764699e+01
,6.119593651161370929e+01
,6.130381056150076802e+01
,6.141623425248351964e+01
,6.153318976033322230e+01
,6.165465854232363796e+01
,6.178062134017088169e+01
,6.191105818308675879e+01
,6.204594839094500003e+01
,6.218527057756000431e+01
,6.232900265407754148e+01
,6.247712183247679718e+01
,6.262960462918333349e+01
,6.278642686879224755e+01
,6.294756368790112333e+01
,6.311298953905198061e+01
,6.328267819478165990e+01
,6.345660275178013165e+01
,6.363473563515582043e+01
,6.381704860280752456e+01
,6.400351274990200068e+01
,6.419409851345675122e+01
,6.438877567702704141e+01
,6.458751337549658444e+01
,6.479028009997108484e+01
,6.499704370277376597e+01
,6.520777140254226367e+01
,6.542242978942593368e+01
,6.564098483038287668e+01
,6.586340187457560091e+01
,6.608964565886483911e+01
,6.631968031340034031e+01
,6.655346936730776974e+01
,6.679097575447114821e+01
,6.703216181940931051e+01
,6.727698932324616976e+01
,6.752541944977322430e+01
,6.777741281160365361e+01
,6.803292945641705103e+01
,6.829192887329365647e+01
,6.855436999913717955e+01
,6.882021122518509060e+01
,6.908941040360552677e+01
,6.936192485417969067e+01
,6.963771137106846254e+01
,6.991672622966255801e+01
,7.019892519351486726e+01
,7.048426352135386708e+01
,7.077269597417718217e+01
,7.106417682242395983e+01
,7.135865985322510596e+01
,7.165609837773013169e+01
,7.195644523850940288e+01
,7.225965281703066978e+01
,7.256567304120891038e+01
,7.287445739302776815e+01
,7.318595691623194455e+01
,7.350012222408889784e+01
,7.381690350721893878e+01
,7.413625054149227367e+01
,7.445811269599167304e+01
,7.478243894103989930e+01
,7.510917785629017374e+01
,7.543827763887856008e+01
,7.576968611163709966e+01
,7.610335073136631934e+01
,7.643921859716556355e+01
,7.677723645882024073e+01
,7.711735072524444945e+01
,7.745950747297743533e+01
,7.780365245473305436e+01
,7.814973110800031009e+01
,7.849768856369395564e+01
,7.884746965485386738e+01
,7.919901892539132859e+01
,7.955228063888137058e+01
,7.990719878739963633e+01
,8.026371710040196206e+01
,8.062177905364592334e+01
,8.098132787815249856e+01
,8.134230656920641422e+01
,8.170465789539399282e+01
,8.206832440767706771e+01
,8.243324844850094735e+01
,8.279937216093608754e+01
,8.316663749785072923e+01
,8.353498623111411803e+01
,8.390435996082837278e+01
,8.427470012458735482e+01
,8.464594800676145780e+01
,8.501804474780686860e+01
,8.539093135359725295e+01
,8.576454870477704162e+01
,8.613883756613464016e+01
,8.651373859599370064e+01
,8.688919235562154597e+01
,8.726513931865295604e+01
,8.764151988052761055e+01
,8.801827436794032167e+01
,8.839534304830171152e+01
,8.877266613920869531e+01
,8.915018381792266666e+01
,8.952783623085402098e+01
,8.990556350305168110e+01
,9.028330574769609029e+01
,9.066100307559379701e+01
,9.103859560467269318e+01
,9.141602346947605895e+01
,9.179322683065383615e+01
,9.217014588444995127e+01
,9.254672087218391141e+01
,9.292289208972513848e+01
,9.329859989695897582e+01
,9.367378472724210781e+01
,9.404838709684666753e+01
,9.442234761439118529e+01
,9.479560699025664405e+01
,9.516810604598659040e+01
,9.553978572366972344e+01
,9.591058709530307169e+01
,9.628045137213479165e+01
,9.664931991398495370e+01
,9.701713423854246798e+01
,9.738383603063726923e+01
,9.774936715148598410e+01
,9.811366964790934730e+01
,9.847668576152059927e+01
,9.883835793788269086e+01
,9.919862883563321532e+01
,9.955744133557577413e+01
,9.991473854973577318e+01
,1.002704638303797822e+02
,1.006245607789968517e+02
,1.009769732552400683e+02
,1.013276453858274238e+02
,1.016765215734003078e+02
,1.020235465053380608e+02
,1.023686651625275488e+02
,1.027118228280863690e+02
,1.030529650960376102e+02
,1.033920378799359980e+02
,1.037289874214427670e+02
,1.040637602988489050e+02
,1.043963034355449793e+02
,1.047265641084359942e+02
,1.050544899563004861e+02
,1.053800289880921923e+02
,1.057031295911828863e+02
,1.060237405395452583e+02
,1.063418110018747882e+02
,1.066572905496485077e+02
,1.069701291651203832e+02
,1.072802772492513697e+02
,1.075876856295729880e+02
,1.078923055679835556e+02
,1.081940887684752113e+02
,1.084929873847910500e+02
,1.087889540280110197e+02
,1.090819417740650863e+02
,1.093719041711728153e+02
,1.096587952472081895e+02
,1.099425695169881578e+02
,1.102231819894840896e+02
,1.105005881749549985e+02
,1.107747440920010007e+02
,1.110456062745364960e+02
,1.113131317786814520e+02
,1.115772781895699239e+02
,1.118380036280747731e+02
,1.120952667574473622e+02
,1.123490267898712887e+02
,1.125992434929291335e+02
,1.128458771959811031e+02
,1.130888887964545688e+02
,1.133282397660436800e+02
,1.135638921568177437e+02
,1.137958086072377313e+02
,1.140239523480798027e+02
,1.142482872082647987e+02
,1.144687776205930732e+02
,1.146853886273834604e+02
,1.148980858860156360e+02
,1.151068356743750201e+02
,1.153116048961991993e+02
,1.155123610863251855e+02
,1.157090724158366584e+02
,1.159017076971102398e+02
,1.160902363887601183e+02
,1.162746286004803267e+02
,1.164548550977836072e+02
,1.166308873066364526e+02
,1.168026973179893417e+02
,1.169702578922016585e+02
,1.171335424633602855e+02
,1.172925251434916305e+02
,1.174471807266659198e+02
,1.175974846929935751e+02
,1.177434132125126496e+02
,1.178849431489669968e+02
,1.180220520634743906e+02
,1.181547182180840991e+02
,1.182829205792233438e+02
,1.184066388210320611e+02
,1.185258533285854980e+02
,1.186405452010039738e+02
,1.187506962544496503e+02
,1.188562890250094028e+02
,1.189573067714636352e+02
,1.190537334779405683e+02
,1.191455538564554786e+02
,1.192327533493344447e+02
,1.193153181315225453e+02
,1.193932351127755851e+02
,1.194664919397356329e+02
,1.195350769978894903e+02
,1.195989794134102056e+02
,1.196581890548809355e+02
,1.197126965349013972e+02
,1.197624932115760146e+02
,1.198075711898842428e+02
,1.198479233229321039e+02
,1.198835432130854031e+02
,1.199144252129840140e+02
,1.199405644264372626e+02
,1.199619567092001091e+02
,1.199785986696303155e+02
,1.199904876692260842e+02
,1.199976218230444260e+02
,1.200000000000000000e+02
};

static void syncWithVisionSystem(void)
{
    aligned_total_yaw = BUFUpdata(buffer_yaw, gimbal_fetch_data.yaw, 1);
    aligned_total_pitch = BUFUpdata(buffer_pitch, gimbal_fetch_data.pitch, 1);
}

void cmd_init()
{
    K230_data = K230ProtocolInit(&huart1, syncWithVisionSystem);
    // laser_c laser_config = {
    //     .laser_tim_driver = &htim3,
    //     .Channel = TIM_CHANNEL_3
    // };
    // Laser = laser_init(&laser_config);
    // laser_disable();
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_RESET);
    buffer_yaw = BUFRegister();
    buffer_pitch = BUFRegister();
    chassis_cmd_pub = PubRegister("chassis_control", sizeof(chassis_control_t));
    chassis_feed_sub = SubRegister("chassis_state", sizeof(chassis_state_t));
    gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
    gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    cmd_ui_sub = SubRegister("ui_ctrl_cmd",sizeof(CMD_Ctrl_UI_s));
    cmd_ui_pub = PubRegister("cmd_feed_ui",sizeof(CMD_Upload_UI_s));
    OLED_ClearArea(10, 4, 96, 30);
    OLED_ShowString(10, 4, "CMD OK!", OLED_6X8);
    OLED_ShowString(10, 14, "Gimbal Init...", OLED_6X8);
	OLED_DrawRectangle(0, 40, 48, 15, OLED_FILLED);
    OLED_DrawRectangle(0, 40, 96, 15, OLED_UNFILLED);
	OLED_Update();
}

static void K230_todo()
{
    if(K230_data->color_det.w == 1)
    {
        if(K230_data->color_det.x < 320 &&
            K230_data->color_det.y < 240 && 
            K230_data->color_det.x > -320 && 
            K230_data->color_det.y > -240 && 
            (K230_data->color_det.x || K230_data->color_det.y))
        {
            if(K230_data->color_det.x < 5 && K230_data->color_det.x > -5 && K230_data->color_det.y < 5 && K230_data->color_det.y > -5)
            {
                // laser_enable();
                HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET);
            }else{
                // laser_disable();
                // HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_RESET);
            }
            yaw_cmd = 0.15 * K230_data->color_det.x + aligned_total_yaw;
            pitch_cmd = - 0.12 * K230_data->color_det.y + aligned_total_pitch;
        }else
        {
            if(HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_8) == GPIO_PIN_RESET)
            {
                if(K230_data->color_det.h == 0)
                {
                    yaw_cmd += 4 * 0.4 / 50;
                    pitch_cmd = -10;
                }else
                if(K230_data->color_det.h == 2)
                {
                    yaw_cmd -= 4 * 0.4 / 50;
                    pitch_cmd = -10;
                }
                
            }
        
        }
    }
    
    pitch_cmd = pitch_cmd < -60 ? -60 : pitch_cmd;
    pitch_cmd = pitch_cmd > 30 ? 30 : pitch_cmd;
}

static void task_sin()
{
    static uint32_t time,time2;
    time++;
    time2++;
    time %= 250;
    time2 %= 100;
    pitch_cmd = (pitch_sin[5 * time2] - 90) / 5;
    yaw_cmd = (yaw_sin[2 * time] - 90) / 10 + 30;
    gimbal_cmd_send.gimbal_task = 1;
}

static void task_circle()
{
    static uint16_t time;
    time++;
    time %= 500;
    pitch_cmd = pitch_circle[time]/10;
    yaw_cmd = yaw_circle[time]/10;
}

void cmd_task()
{
    SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
    SubGetMessage(chassis_feed_sub, &chassis_fetch_data);
    SubGetMessage(cmd_ui_sub,&cmd_ui_recv);
    // YAKp = cmd_ui_recv.yaw_motor.AKp;
    // YSKp = cmd_ui_recv.yaw_motor.SKp;
    // PAKp = cmd_ui_recv.pitch_motor.AKp;
    // PSKp = cmd_ui_recv.pitch_motor.SKp;
    
    K230_todo();
    // task_sin();
    // task_circle();
    gimbal_cmd_send.pitch = pitch_cmd;
    gimbal_cmd_send.yaw = yaw_cmd;
    // gimbal_cmd_send.yaw_motor.AKp = YAKp;
    // gimbal_cmd_send.yaw_motor.SKp = YSKp;
    // gimbal_cmd_send.pitch_motor.AKp = PAKp;
    // gimbal_cmd_send.pitch_motor.SKp = PSKp;
    // cmd_feedback_ui.k230_pitch = K230_data->color_det.y;
    // cmd_feedback_ui.k230_yaw = K230_data->color_det.x;
    PubPushMessage(chassis_cmd_pub, &chassis_cmd_send);
    PubPushMessage(gimbal_cmd_pub, &gimbal_cmd_send);
    PubPushMessage(cmd_ui_pub,&cmd_feedback_ui);
}